#include "xd_misc.h"

static unsigned long start_time,timeout_value,overflow_cnt,timeout_flag;
static unsigned long last_time_diff,time_diff,time_diff,cur_time;

void xd_sm_start_timer(unsigned long time_value)
{
	timeout_value = time_value;
	overflow_cnt = 0;
	last_time_diff = 0;
	timeout_flag = 0;
	start_time = xd_sm_get_timer_tick();
}

int xd_sm_check_timer()
{
	cur_time = xd_sm_get_timer_tick();
	
	if(cur_time < start_time)
	{
		time_diff = XD_SM_MAX_TIMER_TICK - start_time + cur_time + 1;
	}
	else
	{
		time_diff = cur_time - start_time;
	}
	
	if(last_time_diff > time_diff)
	{
		overflow_cnt++;
	}
	last_time_diff = time_diff;

#ifdef AML_ATHENA	
	time_diff += (overflow_cnt << 24);
#else
	time_diff += (overflow_cnt << 16);
#endif
	
	if(time_diff >= timeout_value)
	{
		timeout_flag = 1;
		return 1;
	}
	else
	{
		return 0;
	}
}

int xd_sm_check_timeout()
{
	return timeout_flag;
}

void xd_sm_delay_100ns(unsigned long num_100ns)
{
	unsigned long i;
	for(i = 0; i < num_100ns; i++)
	{
		__asm__("nop");
		__asm__("nop");
		__asm__("nop");
		__asm__("nop");
		__asm__("nop");
		__asm__("nop");
		__asm__("nop");
#if (defined T3510) || (defined T3511)				//For 3295, not need this one
		__asm__("nop");
#endif
	}
}

void xd_sm_delay_us(unsigned long num_us)
{
	udelay(num_us);
}

void xd_sm_delay_ms(unsigned long num_ms)
{
	unsigned long i;
	for(i = 0; i < num_ms; i++)
	{
#ifdef AML_ATHENA
		msleep(1);
#else
		xd_sm_delay_us(1000);
#endif
	}
}

//ECC routines
unsigned char ecc_table[256] = {
	0x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F, 0x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03, 0x56, 0x55, 0x00,
	0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A, 0x69, 0x3C, 0x66, 0x33, 0x30, 0x65,
	0x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69, 0x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65, 0x30, 0x33, 0x66,
	0x03, 0x56, 0x55, 0x00, 0x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03,
	0x69, 0x3C, 0x3F, 0x6A, 0x30, 0x65, 0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F, 0x3C, 0x69,
	0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56, 0x03, 0x00, 0x55, 0x0F, 0x5A, 0x59, 0x0C,
	0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55, 0x55, 0x00, 0x03, 0x56, 0x0C, 0x59, 0x5A, 0x0F,
	0x6A, 0x3F, 0x3C, 0x69, 0x33, 0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F, 0x6A,
	0x6A, 0x3F, 0x3C, 0x69, 0x33, 0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3C, 0x3F, 0x6A,
	0x0F, 0x5A, 0x59, 0x0C, 0x56, 0x03, 0x00, 0x55, 0x55, 0x00, 0x03, 0x56, 0x0C, 0x59, 0x5A, 0x0F,
	0x0C, 0x59, 0x5A, 0x0F, 0x55, 0x00, 0x03, 0x56, 0x56, 0x03, 0x00, 0x55, 0x0F, 0x5A, 0x59, 0x0C,
	0x69, 0x3C, 0x3F, 0x6A, 0x30, 0x65, 0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6A, 0x3F, 0x3C, 0x69,
	0x03, 0x56, 0x55, 0x00, 0x5A, 0x0F, 0x0C, 0x59, 0x59, 0x0C, 0x0F, 0x5A, 0x00, 0x55, 0x56, 0x03,
	0x66, 0x33, 0x30, 0x65, 0x3F, 0x6A, 0x69, 0x3C, 0x3C, 0x69, 0x6A, 0x3F, 0x65, 0x30, 0x33, 0x66,
	0x65, 0x30, 0x33, 0x66, 0x3C, 0x69, 0x6A, 0x3F, 0x3F, 0x6A, 0x69, 0x3C, 0x66, 0x33, 0x30, 0x65,
	0x00, 0x55, 0x56, 0x03, 0x59, 0x0C, 0x0F, 0x5A, 0x5A, 0x0F, 0x0C, 0x59, 0x03, 0x56, 0x55, 0x00
};

#if 0
unsigned char cis_data[] = {
	0x01, 0x03, 0xD9, 0x01, 0xFF, 0x18, 0x02, 0xDF, 0x01, 0x20, 0x04, 0x00, 0x00, 0x00, 0x00, 0x21,
	0x02, 0x04, 0x01, 0x22, 0x02, 0x01, 0x01, 0x22, 0x03, 0x02, 0x04, 0x07, 0x1A, 0x05, 0x01, 0x03,
	0x00, 0x02, 0x0F, 0x1B, 0x08, 0xC0, 0xC0, 0xA1, 0x01, 0x55, 0x08, 0x00, 0x20, 0x1B, 0x0A, 0xc1,
	0x41, 0x99, 0x01, 0x55, 0x64, 0xF0, 0xFF, 0xFF, 0x20, 0x1B, 0x0C, 0x82, 0x41, 0x18, 0xEA, 0x61,
	0xF0, 0x01, 0x07, 0xF6, 0x03, 0x01, 0xEE, 0x1B, 0x0C, 0x83, 0x41, 0x18, 0xEA, 0x61, 0x70, 0x01,
	0x07, 0x76, 0x03, 0x01, 0xEE, 0x15, 0x14, 0x05, 0x00, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
	0x00, 0x20, 0x20, 0x20, 0x20, 0x00, 0x30, 0x2E, 0x30, 0x00, 0xFF, 0x14, 0x00, 0xFF, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x0C, 0xCC, 0xC3
};
#endif

#define BIT7			0x80
#define BIT6			0x40
#define BIT2			0x04
#define BIT0			0x01
#define BIT1BIT0		0x03
#define BIT23			0x00800000
#define MASK_CPS		0x3F
#define CORRECTABLE		0x00555554

// Transfer result
// LP14, 12, 10,... & LP15, 13, 11,... -> LP15, 14, 13,... & LP7, 6, 5,...
// reg2: LP14, 12, 10,...
// reg3: LP15, 13, 11,...
// ecc1: LP15, 14, 13,...
// ecc2: LP07, 06, 05,...
void ecc_trans_result(unsigned char reg2, unsigned char reg3, unsigned char *ecc1, unsigned char *ecc2)
{
    unsigned char a;					// Working for reg2, reg3
    unsigned char b;					// Working for ecc1, ecc2
    unsigned char i;					// For counting
    
    a = BIT7; b = BIT7;
    *ecc1 = *ecc2 = 0;					// Clear ecc1, ecc2
    for(i=0; i<4; ++i)
    {
        if((reg3 & a) != 0)				// LP15, 13, 11, 9 --> ecc1
            *ecc1 |= b;
        b >>= 1;						// Right shift
        if((reg2 & a) != 0)				// LP14, 12, 10, 8 --> ecc1
            *ecc1 |= b;
        b >>= 1;						// Right shift
        a >>= 1;						// Right shift
    }
    b = BIT7;
    for(i=0; i<4; ++i)
    {
        if((reg3 & a) != 0)				// LP7, 5, 3, 1 --> ecc2
            *ecc2 |= b;
        b >>= 1;						// Right shift
        if((reg2 & a) != 0)				// LP6, 4, 2, 0 --> ecc2
            *ecc2 |= b;
        b >>= 1;						// Right shift
        a >>= 1;						// Right shift
    }
}

// Calculating ECC
// data[0-255] -> ecc1, ecc2, ecc3 using CP0-CP5 code table[0-255]
// table: CP0-CP5 code table
// data: DATA
// ecc1: LP15, 14, 13,...
// ecc2: LP07, 06, 05,...
// ecc3: CP5, CP4, CP3,... "1", "1"
void ecc_calculate_ecc(unsigned char *table, unsigned char *data, unsigned char *ecc1, unsigned char *ecc2, unsigned char *ecc3)
{
    unsigned int i;						// For counting
    unsigned char a;					// Working for table
    unsigned char reg1;					// D-all, CP5, CP4, CP3,...
    unsigned char reg2;					// LP14, 12, 10,...
    unsigned char reg3;					// LP15, 13, 11,...
    
    reg1 = reg2 = reg3 = 0;				// Clear parameter
    for(i=0; i<256; i++)
    {
        a = table[data[i]];				// Get CP0-CP5 code from table
        reg1 ^= (a & MASK_CPS);			// XOR with a
        if((a & BIT6) != 0)				// if D-all(all bit XOR) = 1
        {
            reg3 ^= (unsigned char)i;	// XOR with counter
            reg2 ^= ~((unsigned char)i);// XOR with inv. of counter
        }
    }
    
    // Trans LP14, 12, 10,... & LP15, 13, 11,... -> LP15, 14, 13,... & LP7, 6, 5,...
    ecc_trans_result(reg2,reg3,ecc1,ecc2);

    *ecc1 = ~(*ecc1); *ecc2 = ~(*ecc2);	// Inv. ecc2 & ecc3
    *ecc3 = ((~reg1) << 2) | BIT1BIT0;	// Make TEL format
}

// data: DATA
// ecc1: LP15, 14, 13,...
// ecc2: LP07, 06, 05,...
// ecc3: CP5, CP4, CP3,... "1", "1"
unsigned char ecc_correct_data(unsigned char *data, unsigned char *data_ecc, unsigned char ecc1, unsigned char ecc2, unsigned char ecc3)
{
	unsigned long l;					// Working to check d
	unsigned long d;					// Result for comparison
	unsigned int i;						// For counting
	unsigned char d1, d2, d3;			// Result for comparison
	unsigned char a;					// Working for add
	unsigned char add;					// Byte address of cor. DATA
	unsigned char b;					// Working for bit
	unsigned char bit;					// Bit address of cor. DATA
	
	d1 = ecc1 ^ data_ecc[1];//data[257];	// Compare LP's
	d2 = ecc2 ^ data_ecc[0];//data[256];
	d3 = ecc3 ^ data_ecc[2];//data[258];	// Compare CP's
	d = ((unsigned long)d1 << 16) + ((unsigned long)d2 << 8) + (unsigned long)d3; // Result for comparison
	
	if(d == 0)							// if No error, return
		return 0;
		
	if(((d ^ (d >> 1)) & CORRECTABLE) == CORRECTABLE)	// if correctable
	{
		l = BIT23;
		add = 0;						// Clear parameter
		a = BIT7;
		for(i=0; i<8; ++i)				// Checking 8 bit
		{
			if((d & l) != 0)			// Make byte address from LP's
				add |= a;
			l >>= 2;					// Right shift
			a >>= 1;
		}
		bit = 0;						// Clear parameter
		b = BIT2;
		for(i=0; i<3; ++i)				// Checking 3 bit
		{
			if((d & l) != 0)			// Make byte address from LP's
				bit |= b;
			l >>= 2;					// Right shift
			b >>= 1;
		}
		b = BIT0;
		data[add] ^= (b << bit);		// Put corrected data
		
		return 1;
	}
	
	i = 0;								// Clear count
	d &= 0x00FFFFFF;					// Masking
	while(d)							// if d=0 finish counting
	{
		if(d & BIT0)					// Clear number of 1 bit				
			++i;
		d >>= 1;						// Right shift
	}
	if(i == 1)							// if ECC error
	{
		//data[257] = ecc1;				// Put right ECC code
		//data[256] = ecc2;
		//data[258] = ecc3;
		data_ecc[1] = ecc1;				// Put right ECC code
		data_ecc[0] = ecc2;
		data_ecc[2] = ecc3;
		
		return 2;						// ECC error
	}
	
	return 3;							// Uncorrectable error
}
